#include <gazebo/gazebo_client.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/msgs/scene.pb.h>
#include <iostream>
#include <mutex>
#include <condition_variable>
#include <algorithm> // 添加transform函数所需头文件

#include <gazebo/msgs/vector3d.pb.h>
#include <gazebo/msgs/pose.pb.h>
#include <ignition/math/Vector3.hh>

// 使用同步机制确保接收到响应
std::mutex mtx;
std::condition_variable cv;
bool response_received = false;

// 全局模型修改发布者
gazebo::transport::PublisherPtr modelModifyPub = nullptr;

// 回调函数
void responseCallback(const boost::shared_ptr<const gazebo::msgs::Response> &msg)
{
    std::lock_guard<std::mutex> lock(mtx);

    std::cout << "收到响应，类型: " << msg->type() << std::endl;

    if (msg->type() == "gazebo.msgs.Scene")
    {
        // 解析场景信息
        gazebo::msgs::Scene scene;
        scene.ParseFromString(msg->serialized_data());

        std::cout << "\n===== 检测到仿真模型 =====" << std::endl;
        std::cout << "场景名称: " << scene.name() << std::endl;
        std::cout << "模型数量: " << scene.model_size() << "\n"
                  << std::endl;

        // 遍历所有模型
        for (int i = 0; i < scene.model_size(); ++i)
        {
            const gazebo::msgs::Model &model = scene.model(i);
            const gazebo::msgs::Pose &pose = model.pose();

            std::cout << "模型 " << i + 1 << ": " << model.name() << std::endl;
            std::cout << "  位置: ("
                      << pose.position().x() << ", "
                      << pose.position().y() << ", "
                      << pose.position().z() << ")" << std::endl;
            std::cout << "  姿态: ("
                      << pose.orientation().w() << ", "
                      << pose.orientation().x() << ", "
                      << pose.orientation().y() << ", "
                      << pose.orientation().z() << ")" << std::endl;

            // 检查模型名称是否包含"box"（不区分大小写）
            std::string model_name_lower = model.name();
            std::transform(model_name_lower.begin(), model_name_lower.end(), model_name_lower.begin(), ::tolower);
            if (model_name_lower.find("box") != std::string::npos)
            {
                std::cout << ">>> 检测到模型名称包含 'box'，移动该模型。" << std::endl;
                std::cout << ">>> 移动前位置: ("
                          << pose.position().x() << ", "
                          << pose.position().y() << ", "
                          << pose.position().z() << ")" << std::endl;

                // 1. 提取姿态信息并创建四元数
                ignition::math::Quaterniond quat(
                    pose.orientation().w(),
                    pose.orientation().x(),
                    pose.orientation().y(),
                    pose.orientation().z());

                // 2. 在模型坐标系中定义位移向量
                // X轴方向：-0.5m (模型左侧)
                // Y轴方向：+0.5m (模型前方)
                ignition::math::Vector3d local_displacement(-0.5, 0.5, 0.0);

                // 3. 将局部位移转换到世界坐标系
                ignition::math::Vector3d world_displacement = quat.RotateVector(local_displacement);

                // 4. 计算新的世界位置
                ignition::math::Vector3d new_position(
                    pose.position().x() + world_displacement.X(),
                    pose.position().y() + world_displacement.Y(),
                    pose.position().z() + world_displacement.Z());

                std::cout << ">>> 模型坐标系位移: ("
                          << local_displacement.X() << ", "
                          << local_displacement.Y() << ", "
                          << local_displacement.Z() << ")" << std::endl;
                std::cout << ">>> 世界坐标系位移: ("
                          << world_displacement.X() << ", "
                          << world_displacement.Y() << ", "
                          << world_displacement.Z() << ")" << std::endl;
                std::cout << ">>> 移动后位置: ("
                          << new_position.X() << ", "
                          << new_position.Y() << ", "
                          << new_position.Z() << ")" << std::endl;

                // 构造模型修改消息
                gazebo::msgs::Model model_msg;
                model_msg.set_name(model.name());
                gazebo::msgs::Pose *new_pose = model_msg.mutable_pose();
                new_pose->mutable_position()->set_x(new_position.X());
                new_pose->mutable_position()->set_y(new_position.Y());
                new_pose->mutable_position()->set_z(new_position.Z());

                // 保持原有姿态
                new_pose->mutable_orientation()->set_w(pose.orientation().w());
                new_pose->mutable_orientation()->set_x(pose.orientation().x());
                new_pose->mutable_orientation()->set_y(pose.orientation().y());
                new_pose->mutable_orientation()->set_z(pose.orientation().z());

                // 发布模型修改消息
                if (modelModifyPub && modelModifyPub->HasConnections())
                {
                    modelModifyPub->Publish(model_msg);
                    std::cout << ">>> 已发布移动请求。" << std::endl;
                }
                else
                {
                    std::cerr << ">>> 错误：模型修改发布者未连接！" << std::endl;
                }
            }

            std::cout << "---------------------------------" << std::endl;
        }
        std::cout << "=================================\n"
                  << std::endl;
    }

    response_received = true;
    cv.notify_one();
}

int main(int _argc, char **_argv)
{
    gazebo::client::setup(_argc, _argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();

    gazebo::transport::PublisherPtr requestPub =
        node->Advertise<gazebo::msgs::Request>("/gazebo/default/request");

    // 创建模型修改发布者
    modelModifyPub = node->Advertise<gazebo::msgs::Model>("~/model/modify");

    gazebo::transport::SubscriberPtr responseSub =
        node->Subscribe("/gazebo/default/response", responseCallback);

    // 等待连接
    while (!requestPub->WaitForConnection(gazebo::common::Time(5, 0)))
    {
        std::cout << "等待连接到 /gazebo/default/request ..." << std::endl;
    }

    // 等待模型修改发布者连接
    while (!modelModifyPub->WaitForConnection(gazebo::common::Time(5, 0)))
    {
        std::cout << "等待连接到 ~/model/modify ..." << std::endl;
    }

    gazebo::msgs::Request request;
    request.set_request("scene_info");
    request.set_id(0);

    std::cout << "正在请求 Gazebo 场景信息..." << std::endl;
    requestPub->Publish(request);

    // 等待响应处理完成
    {
        std::unique_lock<std::mutex> lock(mtx);
        cv.wait(lock, []
                { return response_received; });
    }

    gazebo::common::Time::MSleep(1000); // 留出时间处理移动请求
    gazebo::client::shutdown();
    return 0;
}
